import numpy as np
from utils import *
from my_A_star_algorithm import *
import cv2
import matplotlib.pyplot as plt


def modify_map(data_array):
    index=np.where(data_array[0,:,:]==1)
    for i in range(len(index[0])):
        num_of_wall=np.sum(data_array[1:5,index[0][i],index[1][i]],axis=0)
        if num_of_wall==3:
            pass
        elif num_of_wall==2:
            for j in range(1,5):
                if data_array[j,index[0][i],index[1][i]]==0:
                    data_array[j, index[0][i], index[1][i]]=1
                    if j==1:
                        data_array[3,index[0][i]-1,index[1][i]]=1
                    elif j==2:
                        data_array[4,index[0][i],index[1][i]-1]=1 
                    elif j==3:
                        data_array[1,index[0][i]+1,index[1][i]]=1 
                    elif j==4:
                        data_array[2,index[0][i],index[1][i]+1]=1 
                    break
    return data_array
def verify_map(data_array,treasure_num,is_symmetry):
    start_point=[9,0]
    end_point=[0,9]
    try:
        aStar = AStar(data_array, Point(start_point[0], start_point[1]), Point(end_point[0], end_point[1]))
        pathList = aStar.start()
    except:
        print("astar error")
        pathList=None
    if pathList==None:
        return 0
    index=np.where(data_array[0,:,:]==1)
    if len(index[0])!=treasure_num:
        print("treasurenum is not right")
        return 0
    for i in range(9):
        for j in range(9):
            if data_array[3,i,j]+data_array[4,i,j]+data_array[1,i+1,j+1]+data_array[2,i+1,j+1]==0:
                print("map wall error")
                return 0
    if is_symmetry:
        for i in range(5):
            for j in range(5):
                if data_array[0,i,j] !=data_array[0,9-i,9-j]:
                    print(1)
                    return 0
        testarray=data_array[1,1:10,0:10]
        for i in range(4):
            for j in range(5):
                if testarray[i,j] !=testarray[8-i,9-j]:
                    print(2)
                    return 0
        testarray = data_array[2, 0:10, 1:10]
        for i in range(5):
            for j in range(4):
                if testarray[i, j] != testarray[9 - i, 8 - j]:
                    print(3)
                    return 0
        testarray = data_array[3, 0:9, 0:10]
        for i in range(4):
            for j in range(5):
                if testarray[i, j] != testarray[8 - i, 9 - j]:
                    print(4)
                    return 0
        testarray = data_array[4, 0:10, 0:9]
        for i in range(5):
            for j in range(4):
                if testarray[i, j] != testarray[9 - i, 8 - j]:
                    print(5)
                    return 0
    return 1

def map2array(img1):
    if img1[0][0][0].dtype != 'uint8':
        img1 = (img1 * 255).astype("uint8")
    img1_filterred = cv2.GaussianBlur(img1, (3, 3), 0)
    img1_gray = cv2.cvtColor(img1_filterred, cv2.COLOR_RGB2GRAY)
    img1_gray = img1_gray / np.max(img1_gray) * 255
    img1_gray = img1_gray.astype("uint8")
    img1_bin = cv2.Canny(img1_gray, 100, 200, apertureSize=3)
    coordinates = get_4_coordinate(img1_bin)
    if len(coordinates) != 4:
        print('img is error')
        return None,None
    else:
        index_line = []
        coordinates_ndarray_unsorted = np.zeros([4, 2], dtype='int16')
        coordinates_ndarray = np.zeros([4, 2], dtype='int16')
        for i in range(4):
            coordinates_ndarray_unsorted[i, :] = np.mean(coordinates[i], axis=0)

        left_up_point_index = np.argmin(np.sum(coordinates_ndarray_unsorted * np.array([1, 1]), axis=1))
        right_down_point_index = np.argmax(np.sum(coordinates_ndarray_unsorted * np.array([1, 1]), axis=1))
        right_up_point_index = np.argmin(np.sum(coordinates_ndarray_unsorted * np.array([-1, 1]), axis=1))
        left_down_point_index = np.argmax(np.sum(coordinates_ndarray_unsorted * np.array([-1, 1]), axis=1))

        coordinates_ndarray[0, :] = coordinates_ndarray_unsorted[left_up_point_index]#按左上，右上，左下，右下的顺序排序
        coordinates_ndarray[1, :] = coordinates_ndarray_unsorted[right_up_point_index]
        coordinates_ndarray[2, :] = coordinates_ndarray_unsorted[left_down_point_index]
        coordinates_ndarray[3, :] = coordinates_ndarray_unsorted[right_down_point_index]
        # for i in range(4):
        #     cv2.putText(img1_filterred, ("#%d" % i), (coordinates_ndarray[i,0], coordinates_ndarray[i,1]), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 0, 255), 2)

        # width,height = 1200,800
        width = np.sqrt((coordinates_ndarray_unsorted[left_up_point_index][0] -
                         coordinates_ndarray_unsorted[right_up_point_index][0]) ** 2 + (
                                coordinates_ndarray_unsorted[left_up_point_index][1] -
                                coordinates_ndarray_unsorted[right_up_point_index][1]) ** 2).astype('int')
        height = np.sqrt((coordinates_ndarray_unsorted[left_up_point_index][0] -
                          coordinates_ndarray_unsorted[left_down_point_index][0]) ** 2 + (
                                 coordinates_ndarray_unsorted[left_up_point_index][1] -
                                 coordinates_ndarray_unsorted[left_down_point_index][1]) ** 2).astype('int')

        changed_coordinates = np.array([[0, 0], [width, 0], [0, height], [width, height]], dtype='float32')

        coordinates_ndarray = np.array(coordinates_ndarray, dtype='float32')

        M = cv2.getPerspectiveTransform(coordinates_ndarray, changed_coordinates)

        img1_filterred_warped = cv2.warpPerspective(img1_filterred, M, (width, height))

        img1_bin_warped = cv2.warpPerspective(img1_bin, M, (width, height))
        index_bin = np.where(img1_bin_warped != 0)
        img1_bin_warped[index_bin] = 1
        img1_gray_warped = cv2.warpPerspective(img1_gray, M, (width, height))


        contours, _hierarchy = cv2.findContours(img1_bin_warped, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)

        cnt_max = contours[0]
        for cnt in contours:
            if cnt.shape[0] > cnt_max.shape[0]:
                cnt_max = cnt

        cnt_max = cnt_max.squeeze()
        cnt_max_1 = cnt_max * np.array([1, 1])
        cnt_max_1 = np.sum(cnt_max_1, axis=1)
        index_max = np.argmax(cnt_max_1)
        index_min = np.argmin(cnt_max_1)


        coordinates_1 = cnt_max[index_min]
        coordinates_2 = cnt_max[index_max]

        [h_start, h_end, w_start, w_end] = [coordinates_1[1], coordinates_2[1], coordinates_1[0], coordinates_2[0]]
        img_slit = img1_bin_warped[h_start:h_start + 5, w_start:w_end]
        img_slit_sum = np.sum(img_slit, axis=1)
        mat_center = get_line_mat_center(img_slit_sum)
        h_start = h_start + mat_center-1

        img_slit = img1_bin_warped[h_end - 5:h_end, w_start:w_end]
        img_slit_sum = np.sum(img_slit, axis=1)
        mat_center = get_line_mat_center(img_slit_sum)
        h_end = h_end - mat_center+1

        img_slit = img1_bin_warped[h_start:h_end, w_start:w_start + 5]
        img_slit_sum = np.sum(img_slit, axis=0)
        mat_center = get_line_mat_center(img_slit_sum)
        w_start = w_start + mat_center-1

        img_slit = img1_bin_warped[h_start:h_end, w_end - 5:w_end]
        img_slit_sum = np.sum(img_slit, axis=0)
        mat_center = get_line_mat_center(img_slit_sum)
        w_end = w_end - mat_center+1


        if (h_end - h_start - (w_end - w_start) > 20):
            print('something is error')
            return None,None
        else:
            img_bin_croped = img1_bin_warped[h_start:h_end, w_start:w_end]
            img_filterred_croped = img1_filterred_warped[h_start:h_end, w_start:w_end]
            img1_gray_croped = img1_gray_warped[h_start:h_end, w_start:w_end]

            img1_gray_croped_300 = cv2.resize(img1_gray_croped, (300, 300))
            global img_filterred_croped_300
            img_filterred_croped_300 = cv2.resize(img_filterred_croped, (300, 300))
            img_bin_croped_300 = cv2.resize(img_bin_croped, (300, 300), cv2.INTER_NEAREST)

            data_array = np.ones([5, 10, 10])
            data_array_test = np.zeros([10, 10])
            data_array_test1 = np.zeros([10, 10])
            img_group = np.zeros([10, 10, 30, 30])

            for i in range(10):
                for j in range(10):
                    data_array_test[i][j] = np.mean(
                        img1_gray_croped_300[i * 30 + 10:(i + 1) * 30 - 10, j * 30 + 10:(j + 1) * 30 - 10])

                    if (i * 30 - 2 < 0):
                        pass
                    else:
                        data_array_test1[i, j] = np.sum(
                            img_bin_croped_300[i * 30 - 5:i * 30 + 5, j * 30 + 5:(j + 1) * 30 - 5])

            Treasure_threshold = (np.max(data_array_test) + np.min(data_array_test)) / 2
            wall_threshold = (np.max(data_array_test1) // 3)

            for i in range(10):
                for j in range(10):
                    # img_group[i,j,:,:]=img1_gray_croped_300[i*30:(i+1)*30,j*30:(j+1)*30]
                    # plt.imsave('./cut/cut{:1d}{:1d}.jpg'.format(i,j),img1_gray_croped_300[i*30:(i+1)*30,j*30:(j+1)*30],cmap='gray')

                    data_array_test[i][j] = np.min(
                        img1_gray_croped_300[i * 30 + 10:(i + 1) * 30 - 10, j * 30 + 10:(j + 1) * 30 - 10])
                    if (np.min(img1_gray_croped_300[i * 30 + 10:(i + 1) * 30 - 10,
                                j * 30 + 10:(j + 1) * 30 - 10]) < Treasure_threshold):
                        data_array[0, i, j] = 1
                    else:
                        data_array[0, i, j] = 0

                    if (i * 30 - 2 < 0):
                        data_array[1, i, j] = 1
                    elif (np.sum(img_bin_croped_300[i * 30 - 5:i * 30 + 5,
                                 j * 30 + 5:(j + 1) * 30 - 5]) > wall_threshold):
                        data_array[1, i, j] = 1
                    else:
                        data_array[1, i, j] = 0

                    if (j * 30 - 2 < 0):
                        data_array[2, i, j] = 1
                    elif (np.sum(img_bin_croped_300[i * 30 + 5:(i + 1) * 30 - 5,
                                 j * 30 - 5:j * 30 + 5]) > wall_threshold):
                        data_array[2, i, j] = 1
                    else:
                        data_array[2, i, j] = 0

                    if ((i + 1) * 30 + 2 > 300):
                        data_array[3, i, j] = 1
                    elif (np.sum(img_bin_croped_300[(i + 1) * 30 - 5:(i + 1) * 30 + 5,
                                 j * 30 + 5:(j + 1) * 30 - 5]) > wall_threshold):
                        data_array[3, i, j] = 1
                    else:
                        data_array[3, i, j] = 0

                    if ((j + 1) * 30 + 2 > 300):
                        data_array[4, i, j] = 1
                    elif (np.sum(img_bin_croped_300[i * 30 + 5:(i + 1) * 30 - 5,
                                 (j + 1) * 30 - 5:(j + 1) * 30 + 5]) > wall_threshold):
                        data_array[4, i, j,] = 1
                    else:
                        data_array[4, i, j] = 0
        data_array[2,9,0]=0
        data_array[4,0,9]=0
        return data_array, img1_gray_croped_300

def is_node(n_array):
    for i in range(4):
        if n_array[i] + n_array[(i + 1) % 4] == 0:
            return 1
    return 0

def array2senddata(data_array,start_point,end_point):#1直行、2左转、3右转
    node_map = np.zeros([11, 11])
    node_map[0, 10] = 0  # 为出口判断是否转弯的作用
    for i in range(10):
        for j in range(10):
            node_map[i][j] = is_node(data_array[1:5, i, j])  #判断是否为需要发送数据的节点
    aStar = AStar(data_array, Point(start_point[0], start_point[1]), Point(end_point[0], end_point[1]))
    pathList = aStar.start()#
    path_array = np.zeros([len(pathList) + 1, 2])
    path_array[0, 0] = start_point[0]
    path_array[0, 1] = start_point[1]
    for i in range(len(pathList)):#path_array会比pathlist长1个单位
        path_array[i + 1, 0] = pathList[i].i
        path_array[i + 1, 1] = pathList[i].j

    path_array = path_array.astype('int')
    if start_point==[9,0]:
        path_array=np.insert(path_array, 0, [9,-1],axis=0)
    if end_point==[0,9]:
        path_array=np.insert(path_array, len(path_array), [0,10],axis=0)
        
    control_list = []
    control_list.append(1)
    vector_array = np.zeros([path_array.shape[0] - 1, 2])
    for i in range(path_array.shape[0] - 1):
        vector_array[i, :] = path_array[i + 1, :] - path_array[i, :]
    for i in range(vector_array.shape[0] - 1):
        if np.dot(vector_array[i, :], vector_array[i + 1, :].T) == 0:
            if vector_array[i + 1, 0] == -vector_array[i, 1] and vector_array[i + 1, 1] == vector_array[i, 0]:
                control_list.append(2)
            else:
                control_list.append(3)
        else:
            control_list.append(1)
    node_list = []
    send_data = []
    node_list.append(1)
    for i in range(1, len(control_list) + 1):
        node_list.append(node_map[path_array[i][0], path_array[i][1]])
    length = 1

    for i in range(len(control_list)):
        if node_list[i] == 1:
            send_data.append([control_list[i], length])
            length = 1
        else:
            length = length + 1

    for i in range(len(send_data) - 1):
        send_data[i][1] = send_data[i + 1][1]
    send_data[len(send_data) - 1][1] = length# 减去1，即为停在宝藏前一格
    send_data.append([0,0])
    send_data_array = np.array(send_data)
    return send_data_array


def generate_start_end(now_point,data_array):
    min_length=100
    record_i=None
    indexs=np.where(data_array[0,:,:]==1)
    if len(indexs[0])==0:
        print(5555555555555)
        return[0,9]
    for i in range(len(indexs[0])):
        aStar = AStar(data_array, Point(now_point[0], now_point[1]), Point(indexs[0][i], indexs[1][i]))
        pathList = aStar.start()
        if pathList is None:
            continue
        if len(pathList)<min_length:
            min_length=len(pathList)
            record_i=i
    return [indexs[0][record_i],indexs[1][record_i]]


if __name__ == "__main__":
    start_point=[9,0]
    img1 = plt.imread('./shot/28.png')
    data_array, crop_map = map2array(img1)

    end_point = generate_start_end(start_point, data_array)
    end_point=[0,9]
    node_map = np.zeros([11, 11])
    node_map[0,10]=0           #为出口判断是否转弯的作用
    for i in range(10):
        for j in range(10):
            node_map[i][j] = is_node(data_array[1:5, i, j])

    aStar = AStar(data_array, Point(start_point[0], start_point[1]), Point(end_point[0], end_point[1]))
    pathList = aStar.start()
    path_array = np.zeros([len(pathList) + 1, 2])
    path_array[0, 0] = start_point[0]
    path_array[0, 1] = start_point[1]
    for i in range(len(pathList)):
        if i == 0:
            cv2.line(crop_map, (Point(start_point[0], start_point[1]).j * 30 + 15, Point(start_point[0], start_point[1]).i * 30 + 15),
                     (pathList[i].j * 30 + 15, pathList[i].i * 30 + 15), (255, 100, 0), 2)  # 第一横
        else:
            cv2.line(crop_map, (pathList[i-1].j * 30 + 15, pathList[i-1].i * 30 + 15),
                     (pathList[i].j * 30 + 15, pathList[i].i * 30 + 15), (255, 100, 0), 2)

        path_array[i + 1, 0] = pathList[i].i
        path_array[i + 1, 1] = pathList[i].j

    path_array=path_array.astype('int')
    if start_point==[9,0]:
        path_array=np.insert(path_array, 0, [9,-1],axis=0)
        node_map[9,0]=1
    if end_point==[0,9]:
        path_array=np.insert(path_array, len(path_array), [0,10],axis=0)
        node_map[0, 9] = 1

    control_list = []
    control_list_wstep=[]
    vector_array = np.zeros([path_array.shape[0] - 1, 2])
    for i in range(path_array.shape[0] - 1):
        vector_array[i, :] = path_array[i + 1, :] - path_array[i, :]
    for i in range(vector_array.shape[0] - 1):
        if np.dot(vector_array[i, :], vector_array[i + 1, :].T) == 0:
            if vector_array[i + 1, 0] == -vector_array[i, 1] and vector_array[i + 1, 1] == vector_array[i, 0]:
                control_list.append(2)
            else:
                control_list.append(3)
        else:
            control_list.append(1)
    i=1
    node_list=[]
    send_data=[]
    node_list.append(0)
    for i in range(1,len(control_list)+1):
        node_list.append(node_map[path_array[i][0],path_array[i][1]])
    length = 1

    print(node_list)
    for i in range(len(control_list)):
        if node_list[i]==1:
            send_data.append([control_list[i],length])
            length=1
        else:
            length=length+1


    for i in range(len(send_data)-1):
        send_data[i][1]=send_data[i+1][1]
    send_data[len(send_data)-1][1] = length-1#减去2，即为停在宝藏前一格

    send_data_array=np.array(send_data)

    #len(send_data_array)
    print(send_data_array)
    plt.imshow(crop_map)
    plt.show()
